#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <yaml-cpp/yaml.h>


class ImagePublisherNode : public rclcpp::Node {
public:
    ImagePublisherNode() : Node("image_publisher_node"), stopThread(false) {
        // 参数声明
        this->declare_parameter<std::string>("subscribles.read_images.url", "rtsp://192.168.16.188:8554/live");//8554
        std::string image_path = this->get_parameter("subscribles.read_images.url").as_string();
        this->declare_parameter<std::string>("publisher.topic.names", "/airsim_node/drone_0/camera1/Scene");
        std::string topic_name = this->get_parameter("publisher.topic.names").as_string();
        ost_yaml_path = "/home/nvidia/hil_simulate/src/image_publisher_pkg/config/ost.yaml";

        // 加载相机标定文件
        if (!loadCalibrationFile(ost_yaml_path)) {
            RCLCPP_ERROR(this->get_logger(), "Failed to load calibration file");
            rclcpp::shutdown();
        }

        // 创建发布者
        publisher_ = this->create_publisher<sensor_msgs::msg::Image>(topic_name, 10);
        publisher_camera_info = this->create_publisher<sensor_msgs::msg::CameraInfo>(
            "/camera_info", 10);
        RCLCPP_WARN(this->get_logger(), "No frame available to publish33333333333333");
        // 启动帧捕获线程
        captureThread = std::thread(&ImagePublisherNode::captureFrame, this, image_path);

        // 定时器发布最新帧
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100), [this]() {
                cv::Mat frame;
                {
                    if (sharedFrame.empty()) {
                        RCLCPP_WARN(this->get_logger(), "No frame available to publish");
                        return;
                    }
                    frame = sharedFrame.clone();
                    RCLCPP_WARN(this->get_logger(), "get frame");
                }

                // 发布图像
                std_msgs::msg::Header header;
                header.frame_id = "camera";
                header.stamp = this->get_clock()->now();
                auto msg = cv_bridge::CvImage(header, "bgr8", frame).toImageMsg();
                publisher_->publish(*msg);

                // 发布相机信息
                camera_info_msg_.header = header;
                publisher_camera_info->publish(camera_info_msg_);

                RCLCPP_INFO(this->get_logger(), "Published frame");
            });
    }

    ~ImagePublisherNode() {
        stopThread = true;
        if (captureThread.joinable()) {
            captureThread.join();
        }
    }

private:
    std::thread captureThread;
    std::atomic<bool> stopThread;
    std::mutex frameMutex;
    cv::Mat sharedFrame;

    cv::VideoCapture cap_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr publisher_camera_info;
    rclcpp::TimerBase::SharedPtr timer_;
    std::string ost_yaml_path;
    sensor_msgs::msg::CameraInfo camera_info_msg_;

    bool loadCalibrationFile(const std::string &file_path) {
        try {
            YAML::Node config = YAML::LoadFile(file_path);
            camera_info_msg_.width = config["image_width"].as<int>();
            camera_info_msg_.height = config["image_height"].as<int>();
            auto camera_matrix = config["camera_matrix"]["data"].as<std::vector<double>>();
            for (size_t i = 0; i < camera_matrix.size(); ++i) {
                camera_info_msg_.k[i] = camera_matrix[i];
            }
            camera_info_msg_.d = config["distortion_coefficients"]["data"].as<std::vector<double>>();
            camera_info_msg_.distortion_model = config["distortion_model"].as<std::string>();
            auto projection_matrix = config["projection_matrix"]["data"].as<std::vector<double>>();
            for (size_t i = 0; i < projection_matrix.size(); ++i) {
                camera_info_msg_.p[i] = projection_matrix[i];
            }
            camera_info_msg_.r[0] = camera_info_msg_.r[4] = camera_info_msg_.r[8] = 1.0;
            RCLCPP_INFO(this->get_logger(), "Successfully loaded calibration file: %s", file_path.c_str());
            return true;
        } catch (const YAML::Exception &e) {
            RCLCPP_ERROR(this->get_logger(), "Failed to load calibration file: %s", e.what());
            return false;
        }
    }

    void captureFrame(const std::string &rtspUrl) {
       RCLCPP_WARN(this->get_logger(), "No frame available to publish111111");
       RCLCPP_WARN(this->get_logger(),"No frame available to publish %s",rtspUrl.c_str());
        // cv::VideoCapture cap(rtspUrl, cv::CAP_GSTREAMER);
        cv::VideoCapture cap(rtspUrl);
                    RCLCPP_WARN(this->get_logger(), "No frame available to publish44444444444");
        cap.set(cv::CAP_PROP_BUFFERSIZE, 1);
        if (!cap.isOpened()) {

            RCLCPP_ERROR(this->get_logger(), "Failed to open RTSP stream");
            return;
        }
        RCLCPP_WARN(this->get_logger(), "No frame available to publish555555555555555");
        cv::Mat frame;
        while (!stopThread) {
            if (!cap.read(frame)) {
                RCLCPP_WARN(this->get_logger(), "Failed to read frame from RTSP stream");
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            // 更新共享帧
            {
                RCLCPP_WARN(this->get_logger(), "No frame available to publish22222222222");
                std::lock_guard<std::mutex> lock(frameMutex);
                sharedFrame = frame.clone();
            }
           
        }
        cap.release();
    }
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImagePublisherNode>());
    rclcpp::shutdown();
    return 0;
}



